#include "main.h"
#include "BMI088.h"
#include "MyCAN.h"
#include "Kalman.h"
#include "usbd_cdc_if.h"
#include "can.h"
#include "DriversUse.h"
// #include <math.h>

/**
 * @brief 电机CAN发送
 *
 * @param motor
 */
void Userf_MotorActivit(MotorTypedef *motor)
{
    Motor_transmission(motor->SendDate, motor->ID, motor->useCAN);
}

/**
 * @brief 进行电机1的pid调控以及can发送
 *
 */
void Userf_DoubleLoopPIDControl_Motor1(void)
{
    /*         电机1PID          */
    Platform.motor1.g_Speed_pid.Setpoint = FeedForwardPid_closed_control(&Platform.motor1.g_Angle_pid, BMI088.pitch); // 外环输出的速度
    if (Platform.motor1.g_Speed_pid.Setpoint >= 300) {
        Platform.motor1.g_Speed_pid.Setpoint = 300;
    } else if (Platform.motor1.g_Speed_pid.Setpoint <= -300) {
        Platform.motor1.g_Speed_pid.Setpoint = -300;
    }
    Platform.motor1.SendDate = Pid_closed_control(&Platform.motor1.g_Speed_pid, Platform.motor1.ReceiveData.Speed); // 内环输出电压
    if (Platform.motor1.SendDate >= 25000) {
        Platform.motor1.SendDate = 25000;
    } else if (Platform.motor1.SendDate <= -25000) {
        Platform.motor1.SendDate = -25000;
    }
}

/**
 * @brief 进行电机2的pid调控以及can发送
 *
 */
void Userf_DoubleLoopPIDControl_Motor2(void)
{
    /*         电机2PID          */
    Platform.motor2.g_Speed_pid.Setpoint = FeedForwardPid_closed_control(&Platform.motor2.g_Angle_pid, BMI088.yaw); // 外环输出的速度
    if (Platform.motor2.g_Speed_pid.Setpoint >= 300) {
        Platform.motor2.g_Speed_pid.Setpoint = 300;
    } else if (Platform.motor2.g_Speed_pid.Setpoint <= -300) {
        Platform.motor2.g_Speed_pid.Setpoint = -300;
    }
    Platform.motor2.SendDate = Pid_closed_control(&Platform.motor2.g_Speed_pid, Platform.motor2.ReceiveData.Speed); // 内环输出电压
    if (Platform.motor2.SendDate >= 25000) {
        Platform.motor2.SendDate = 25000;
    } else if (Platform.motor2.SendDate <= -25000) {
        Platform.motor2.SendDate = -25000;
    }
}

/**
 * @brief 进行电机1的pid调控以及can发送(没前馈)
 *
 */
void Userf_DoubleLoopPIDControl_Motor1_NOF(void)
{
    /*         电机1PID          */
    Platform.motor1.g_Speed_pid.Setpoint = Pid_closed_control(&Platform.motor1.g_Angle_pid, BMI088.pitch); // 外环输出的速度
    if (Platform.motor1.g_Speed_pid.Setpoint >= 300) {
        Platform.motor1.g_Speed_pid.Setpoint = 300;
    } else if (Platform.motor1.g_Speed_pid.Setpoint <= -300) {
        Platform.motor1.g_Speed_pid.Setpoint = -300;
    }
    Platform.motor1.SendDate = Pid_closed_control(&Platform.motor1.g_Speed_pid, Platform.motor1.ReceiveData.Speed); // 内环输出电压
    if (Platform.motor1.SendDate >= 25000) {
        Platform.motor1.SendDate = 25000;
    } else if (Platform.motor1.SendDate <= -25000) {
        Platform.motor1.SendDate = -25000;
    }
}

/**
 * @brief 进行电机2的pid调控以及can发送(没前馈)
 *
 */
void Userf_DoubleLoopPIDControl_Motor2_NOF(void)
{
    /*         电机2PID          */
    Platform.motor2.g_Speed_pid.Setpoint = Pid_closed_control(&Platform.motor2.g_Angle_pid, BMI088.yaw); // 外环输出的速度
    if (Platform.motor2.g_Speed_pid.Setpoint >= 300) {
        Platform.motor2.g_Speed_pid.Setpoint = 300;
    } else if (Platform.motor2.g_Speed_pid.Setpoint <= -300) {
        Platform.motor2.g_Speed_pid.Setpoint = -300;
    }
    Platform.motor2.SendDate = Pid_closed_control(&Platform.motor2.g_Speed_pid, Platform.motor2.ReceiveData.Speed); // 内环输出电压
    if (Platform.motor2.SendDate >= 25000) {
        Platform.motor2.SendDate = 25000;
    } else if (Platform.motor2.SendDate <= -25000) {
        Platform.motor2.SendDate = -25000;
    }
}

/**
 * @brief 进行电机1的pid调控以及can发送(只使用电机传回的角度，无BMI)
 *
 */
void Userf_DoubleLoopPIDControl_Motor1_UseMotorAngle(void)
{
    /*         电机1PID          */
    Platform.motor1.g_Speed_pid.Setpoint = Pid_closed_control(&Platform.motor1.g_Angle_pid, Platform.motor1.ReceiveData.Angle); // 外环输出的速度
    if (Platform.motor1.g_Speed_pid.Setpoint >= 300) {
        Platform.motor1.g_Speed_pid.Setpoint = 300;
    } else if (Platform.motor1.g_Speed_pid.Setpoint <= -300) {
        Platform.motor1.g_Speed_pid.Setpoint = -300;
    }
    Platform.motor1.SendDate = Pid_closed_control(&Platform.motor1.g_Speed_pid, Platform.motor1.ReceiveData.Speed); // 内环输出电压
    if (Platform.motor1.SendDate >= 25000) {
        Platform.motor1.SendDate = 25000;
    } else if (Platform.motor1.SendDate <= -25000) {
        Platform.motor1.SendDate = -25000;
    }
}

/**
 * @brief 进行电机2的pid调控以及can发送(只使用电机传回的角度，无BMI)
 *
 */
void Userf_DoubleLoopPIDControl_Motor2_UseMotorAngle(void)
{
    /*         电机2PID          */
    Platform.motor2.g_Speed_pid.Setpoint = Pid_closed_control(&Platform.motor2.g_Angle_pid, Platform.motor2.ReceiveData.Angle); // 外环输出的速度
    if (Platform.motor2.g_Speed_pid.Setpoint >= 300) {
        Platform.motor2.g_Speed_pid.Setpoint = 300;
    } else if (Platform.motor2.g_Speed_pid.Setpoint <= -300) {
        Platform.motor2.g_Speed_pid.Setpoint = -300;
    }

    Platform.motor2.SendDate = Pid_closed_control(&Platform.motor2.g_Speed_pid, Platform.motor2.ReceiveData.Speed); // 内环输出电压
    if (Platform.motor2.SendDate >= 25000) {
        Platform.motor2.SendDate = 25000;
    } else if (Platform.motor2.SendDate <= -25000) {
        Platform.motor2.SendDate = -25000;
    }
}

/**
 * @brief 进行两个电机的pid调控以及can发送(只使用电机传回的角度，无BMI)
 *
 */
void Userf_DoubleLoopPIDControl_UseMotorAngle(void)
{
    /*         电机2PID          */
    Platform.motor2.g_Speed_pid.Setpoint = Pid_closed_control(&Platform.motor2.g_Angle_pid, Platform.motor2.ReceiveData.Angle); // 外环输出的速度
    if (Platform.motor2.g_Speed_pid.Setpoint >= 300) {
        Platform.motor2.g_Speed_pid.Setpoint = 300;
    } else if (Platform.motor2.g_Speed_pid.Setpoint <= -300) {
        Platform.motor2.g_Speed_pid.Setpoint = -300;
    }

    Platform.motor2.SendDate = Pid_closed_control(&Platform.motor2.g_Speed_pid, Platform.motor2.ReceiveData.Speed); // 内环输出电压
    if (Platform.motor2.SendDate >= 25000) {
        Platform.motor2.SendDate = 25000;
    } else if (Platform.motor2.SendDate <= -25000) {
        Platform.motor2.SendDate = -25000;
    }

    /*         电机1PID          */
    Platform.motor1.g_Speed_pid.Setpoint = Pid_closed_control(&Platform.motor1.g_Angle_pid, Platform.motor1.ReceiveData.Angle); // 外环输出的速度
    if (Platform.motor1.g_Speed_pid.Setpoint >= 300) {
        Platform.motor1.g_Speed_pid.Setpoint = 300;
    } else if (Platform.motor1.g_Speed_pid.Setpoint <= -300) {
        Platform.motor1.g_Speed_pid.Setpoint = -300;
    }
    Platform.motor1.SendDate = Pid_closed_control(&Platform.motor1.g_Speed_pid, Platform.motor1.ReceiveData.Speed); // 内环输出电压
    if (Platform.motor1.SendDate >= 25000) {
        Platform.motor1.SendDate = 25000;
    } else if (Platform.motor1.SendDate <= -25000) {
        Platform.motor1.SendDate = -25000;
    }
}

/**
 * @brief 电机堵转保护，目前若是电机超过限定角度则产生错误挂起
 *
 */
void Userf_MotorAngleProtect(void)
{
    if (Platform.motor1.ReceiveData.Angle > PROTECTED_ANGLE ||
        Platform.motor1.ReceiveData.Angle < -PROTECTED_ANGLE)
    {
        DebugValue.motor1_ERROR = 1;
        Platform.motor1.SendDate = 0;
        Platform.motor2.SendDate = 0;
        Userf_MotorActivit(&Platform.motor1);
        Userf_MotorActivit(&Platform.motor2);
        Error_Handler();
    }

    if (Platform.motor2.ReceiveData.Angle > PROTECTED_ANGLE ||
        Platform.motor2.ReceiveData.Angle < -PROTECTED_ANGLE)
    {
        DebugValue.motor2_ERROR = 1;
        Platform.motor1.SendDate = 0;
        Platform.motor2.SendDate = 0;
        Userf_MotorActivit(&Platform.motor1);
        Userf_MotorActivit(&Platform.motor2);
        Error_Handler();
    }
}

/**
 * @brief 所有自定义函数的初始化
 *
 */
void Userf_AllInit(void)
{
    /* 两个电机基本参数设置 */
    Platform.motor1.useCAN = &hcan1;
    Platform.motor2.useCAN = &hcan2;
    Platform.motor1.ID     = 1;
    Platform.motor2.ID     = 2;

    /* 发送数据包包头包尾设定 */
    Camera.sendDate[0]  = 0xFF;
    Camera.sendDate[1]  = 0xFF;
    Camera.sendDate[11] = 0xFE;
    Camera.sendDate[12] = 0xFE;

    Camera.red_or_bule = 0;

    /* 设备初始化 */
    BMI088_Init();
    //  quaternion_init(&q);   // 卡尔曼滤波更新
    MyCAN_Init();
    PID_Init();

    /* 等待电机1接收角度0时再初始化BMI的yaw */
    for (uint8_t i = 0; i < 200; i++) {
        Userf_DoubleLoopPIDControl_UseMotorAngle();
        Userf_MotorActivit(&Platform.motor1);
        Userf_MotorActivit(&Platform.motor2);
        Userf_MotorAngleProtect();
        HAL_Delay(1);
    }
    while (Platform.motor2.ReceiveData.Angle > 0.1f || Platform.motor2.ReceiveData.Angle < -0.1f) {
        Userf_DoubleLoopPIDControl_UseMotorAngle();
        Userf_MotorActivit(&Platform.motor1);
        Userf_MotorActivit(&Platform.motor2);
        Userf_MotorAngleProtect();
        HAL_Delay(1);
    }
    
    initialize_BMI088_and_kalman(&KalmanX, &KalmanY, &KalmanZ);
    BMI088_GetGYROzError();
}

/**
 * @brief BMI088结构体更新Pitch和Yaw角
 *
 */
void Userf_BMI088Update(void)
{
    BMI088_ConverData();
    BMI088_Kalman_Filter(&KalmanX, &KalmanY, &KalmanZ, &BMI088);
}

/**
 * @brief USB发送给相机识别红还是蓝灯条
 *
 * @param
 */
void Userf_SendLedTypeToCamera(void)
{
    Camera.txPitch.angle = BMI088.pitch;
    Camera.txYaw.angle   = BMI088.yaw;
    Camera.sendDate[2]   = Camera.red_or_bule;

    Camera.sendDate[3] = Camera.txPitch.date[0];
    Camera.sendDate[4] = Camera.txPitch.date[1];
    Camera.sendDate[5] = Camera.txPitch.date[2];
    Camera.sendDate[6] = Camera.txPitch.date[3];

    Camera.sendDate[7]  = Camera.txYaw.date[0];
    Camera.sendDate[8]  = Camera.txYaw.date[1];
    Camera.sendDate[9]  = Camera.txYaw.date[2];
    Camera.sendDate[10] = Camera.txYaw.date[3];
    CDC_Transmit_FS(Camera.sendDate, 13);
}

// float Userf_SummorSin_Pitch(void)
// {
//     double t = HAL_GetTick() / 1000.0;
//     return 40 * sin(2 * M_PI * t);
// }

// float Userf_SummorSin_Yaw(void)
// {
//     double t = HAL_GetTick() / 1000.0;
//     return 40 * sin(0.2 * M_PI * t);
// }

/**
 * @brief 根据当前实际值重置PID的目标值，用于扫描任务
 * 
 */
void Userf_Reset_PIDSetPoint_Motor(void)
{
    Platform.motor1.g_Angle_pid.Setpoint = Platform.motor1.ReceiveData.Angle;
    Platform.motor2.g_Angle_pid.Setpoint = Platform.motor2.ReceiveData.Angle;
}

/**
 * @brief 根据当前实际值重置PID的目标值，用于自瞄任务
 *
 */
void Userf_Reset_PIDSetPoint_BMI(void)
{
    Platform.motor1.g_Angle_pid.Setpoint = BMI088.pitch;
    Platform.motor2.g_Angle_pid.Setpoint = BMI088.yaw;
}